import time
SIZE = 20+2
MODE = 'm'
class box():
    def __init__(self,x,y,t=0):
        '''
        status:
             0 free
             1 start
             2 end
             3 searched
             4 blocked
        '''
        self.pos = (x,y)
        self.status = t
        self.h = None
        self.g = None
        self.cost = None
        self.father = None
    
    def set_father(self,father):
        self.father = father

    def set_free(self):    self.status = 0
    def set_start(self,end):   
        self.status = 1
        self.g = 0
        self.h = self.dist(end)
        self.cost = self.h
    def set_end(self):
        self.status = 2
        self.h = 0
    def set_searched(self):self.status = 3
    def set_blocked(self): self.status = 4

    def renew_cost(self,father,end):
        if self.status==0: self.status=3
        if self.status==4: return False

        h = self.dist(end,MODE)
        g = father.g+self.dist(father)
        cost = g+h
        if self.status==2: 
            self.set_father(father)
            self.cost = cost
            self.g = g
            return 'done'
        if self.cost == None or cost < self.cost:
            self.cost = cost
            self.h = h
            self.g = g
            self.set_father(father)
            return True
        else: return False

    def dist(a,b,METRIC='e'):
        dx = float(a.pos[0]-b.pos[0])
        dy = float(a.pos[1]-b.pos[1])
        if METRIC == 'e':
            return (dx**2+dy**2)**0.5
        elif METRIC == 'm':
            return abs(dx)+abs(dy)

#map define
court = [[box(x,y,4) for y in range(SIZE)] for x in range(SIZE)]
[[court[x][y].set_free() for y in range(1,SIZE-1)] for x in range(1,SIZE-1)]
[[court[x][y].set_blocked() for y in range(6,12)] for x in range(6,10)]
[[court[x][y].set_blocked() for y in range(10,14)] for x in range(13,18)]

start = court[4][4]
end = court[16][16]
start.set_start(end)
end.set_end()
openlist = []
openlist.append(start)


exit_flag = False
expand_times = 0
start_time = time.time()
while len(openlist)>0:
    k = openlist.pop(0)
    #expand and calculate
    for x in range(-1,2):
        for y in range (-1,2):
            if x==0 and y==0:
                pass
            else:
                pos_x = k.pos[0] + x
                pos_y = k.pos[1] + y
                child = court[pos_x][pos_y]
                r = child.renew_cost(k,end)
                if r:
                    openlist.append(child)
                    expand_times+=1
                if r == 'done':
                    exit_flag=True
                    break
        if exit_flag: break
    if exit_flag: break
                    
    #sort
    openlist.sort(key=lambda node: node.cost)
    pass

#print test
#[print([court[x][y].status for y in range(SIZE)]) for x in range(SIZE)]

route_list = []
is_start = False
node = end
while not is_start:
    route_list.append(node.pos)
    is_start = node.status==1
    node = node.father
end_time = time.time()
#output
mode = 'Euclidean' if MODE == 'e' else 'Manhattan' if MODE == 'm' else 0
print(f'mode: {mode}')
print('route: ',end='')
while True:
    node = route_list.pop()
    print(node,end='')
    if len(route_list)>0:
        print(' -> ',end='')
    else:
        print()
        break
print(f'cost = {end.cost:>.3f}')
print(f'expand = {expand_times}')
print(f'used time = {(end_time-start_time)*1000:.3f}ms')

pass